/*
**Copyright (C) 2022, HITCRT_VISION, all rights reserved.
*/
#include "KF.h"
namespace hitcrt {
/**
 * @brief 状态更新--添加异常处理
 * @param void
 * @return
 * @author www
 * -phone:17390600977;qq:1922039181
 */
void KF::stateUpdate() {
    //**异常处理先不要管
    try {
        checkStateUpdate();
    } catch (const std::exception &ex) {
        std::cerr << ex.what() << std::endl;
        return;
    }
    stateUpdataImpl();
}
/**
 * @brief 测量更新--添加异常处理
 * @param void
 * @return
 * @author www
 * -phone:17390600977;qq:1922039181
 */
void KF::measureUpdate() {
    try {
        checkMeasureUpdate();
    } catch (const std::exception &ex) {
        std::cerr << ex.what() << std::endl;
        return;
    }
    measureUpdateImpl();
}
/**
 * @brief 状态更新方程异常处理
 * @param void
 * @return
 * @author www
 * -phone:17390600977;qq:1922039181
 */
void KF::checkStateUpdate() {
    if (m_transitionMatrix.cols != m_statePost.rows) {
        throw stateUpdateDimErr("first");
    }
    if (m_transitionMatrix.cols != m_errorCovPost.rows ||
        m_errorCovPost.cols != m_transitionMatrix.cols ||
        m_transitionMatrix.rows != m_processNoiseCov.rows ||
        m_transitionMatrix.cols != m_processNoiseCov.cols) {
        throw stateUpdateDimErr("second");
    }
}
/**
 * @brief 测量更新方程异常处理
 * @param void
 * @return
 * @author www
 * -phone:17390600977;qq:1922039181
 */
void KF::checkMeasureUpdate() {
    if (m_measurementMatrix.cols != m_errorCovPrior.rows ||
        m_measurementMatrix.cols != m_errorCovPrior.cols ||
        m_measurementMatrix.rows != m_measurementNoiseCov.rows ||
        m_measurementMatrix.rows != m_measurementNoiseCov.cols)
        throw measUpdateDimErr("first");
    if (m_measurement.rows != m_measurementMatrix.rows ||
        m_measurement.cols != m_statePrior.cols ||
        m_measurementMatrix.cols != m_statePrior.rows ||
        m_statePrior.cols != m_measurement.cols ||
        m_statePrior.rows != m_gain.rows || m_gain.cols != m_measurement.rows)
        throw measUpdateDimErr("second");
    if (m_gain.cols != m_measurementMatrix.rows ||
        m_errorCovPrior.rows != m_measurementMatrix.cols ||
        m_errorCovPrior.cols != m_measurementMatrix.cols ||
        m_gain.cols != m_measurementNoiseCov.rows ||
        m_measurementNoiseCov.cols != m_gain.cols)
        throw measUpdateDimErr("third");
}

void KF::stateUpdataImpl() {

/*
    m_transitionMatrix  -----状态转移矩阵
    m_statePrior        -----预测状态矩阵
    m_statePost         -----当前状态矩阵
    m_errorCovPrior     -----系统不确定性协方差矩阵 更新的
    m_errorCovPost      -----系统不确定性协方差矩阵 上一次的
    m_processNoiseCov   -----噪声协方差矩阵

*/

    /***时间更新方程，卡尔曼滤波为一种高斯滤波，均值和协方差可以表示一个高斯分布*/
    /// X'(k+1|k)=F(k)X(k|k) predict_value  状态更新得到先验证均值（状态）更新
    //std::cout<<"上次状态值: "<<m_statePost<<std::endl;
    // 预测值  =  转换矩阵 * 上次状态值
    m_statePrior = m_transitionMatrix * m_statePost;  // 公式一 计算预测的状态
    //std::cout<<"预测值: "<<m_statePrior<<std::endl;
    /// 状态更新得到 先验协方差估计
    m_errorCovPrior =
        m_transitionMatrix * m_errorCovPost * m_transitionMatrix.t() +
        m_processNoiseCov;  // P预测估计计算

    //std::cout<<"m_errorCovPrior"<<m_errorCovPrior<<std::endl;
    // 用于观测方程残差计算，放在此处便于子类修改变量
    m_transformation = m_measurementMatrix * m_statePrior;  
        
        // m_measurementMatrix =>>
                       // 将预测值转为传感器一个维度的转换矩阵 进而比较
}
void KF::measureUpdateImpl() {
    // 计算中间变量
    const cv::Mat I = cv::Mat::eye(m_stateDimension, m_stateDimension, CV_64F);
    /***状态更新方程*/
    const cv::Mat temp4 =
        m_measurementMatrix * m_errorCovPrior * m_measurementMatrix.t() +
        m_measurementNoiseCov;

    // 卡尔曼增益
    m_gain = m_errorCovPrior * m_measurementMatrix.t() * temp4.inv();  // K`
    //std::cout<<"测试用"<<std::endl;
    /// m_residual=z(k+1)-H(k)X'(k+1|k)  残差
    m_residual = m_measurement - m_transformation;
    //std::cout<<"卡尔曼增益"<<m_gain<<std::endl;
    //std::cout<<"m_errorCovPrior"<<m_errorCovPrior<<std::endl;
    //std::cout<<"m_measurementMatrix.t()"<<m_measurementMatrix.t()<<std::endl;
    //std::cout<<"temp4.inv()"<<temp4.inv()<<std::endl;


    /// 后验状态估计（高斯均值）
    m_statePost = m_statePrior + m_gain * m_residual;

    const cv::Mat I_KH = I - m_gain * m_measurementMatrix;
    /// 后验协方差矩阵
    m_errorCovPost = I_KH * m_errorCovPrior * I_KH.t() +
                     m_gain * m_measurementNoiseCov * m_gain.t();
}
/*完整滤波*/
void KF::predict() {
    stateUpdate();
    measureUpdate();
}
/*用户访问滤波结果*/
const cv::Mat &KF::getResult() { return m_statePost; }
}  // namespace hitcrt
